package com.gitee.wsl.mathematics.coordinate.ext

import com.gitee.wsl.mathematics.number.compareTo
import com.gitee.wsl.mathematics.vector.vec2.Vector2
import kotlin.math.abs


/**
 * Finds the intersection point between two [LineSegment]s.
 *
 * Returns [Vector2.INFINITY] if none exists.
 *
 * @param a0 The start of the first line segment.
 * @param a1 The end of the first line segment.
 * @param b0 The start of the second line segment.
 * @param b1 The end of the second line segment.
 * @param eps How far outside the [t](https://pomax.github.io/bezierinfo/#explanation) value are intersections considered.
 */
fun<N:Number,V:Vector2<N,V>> intersection(a0: V, a1: V, b0: V, b1: V, eps: Double = 0.0): V {
    a1.run {
    val x0 = a0.x
    val x1 = a1.x
    val x2 = b0.x
    val x3 = b1.x

    val y0 = a0.y
    val y1 = a1.y
    val y2 = b0.y
    val y3 = b1.y

    val den = (x0 - x1) * (y2 - y3) - (y0 - y1) * (x2 - x3)

    return if (abs(den) > 10E-6) {
            val px = ((x0 * y1 - y0 * x1) * (x2 - x3) - (x0 - x1) * (x2 * y3 - y2 * x3)) / den
            val py = ((x0 * y1 - y0 * x1) * (y2 - y3) - (y0 - y1) * (x2 * y3 - y2 * x3)) / den

            val s = (-(y1 - y0) * (x0 - x2) + (x1 - x0) * (y0 - y2)) / den
            val t = ((x3 - x2) * (y0 - y2) - (y3 - y2) * (x0 - x2)) / den

            if (t >= 0 - eps && t <= 1 + eps && s >= 0 - eps && s <= 1 + eps) {
                create(px, py)
            } else {
                create(POSITIVE_INFINITY, POSITIVE_INFINITY)
            }
        } else {
           create(POSITIVE_INFINITY, POSITIVE_INFINITY)
        }
    }
}
